/*
 * @Description: Robot chassis motor kinamatics
 * @Version: V1.0
 * @Author: zw_1520@163.com
 * @Date: 2025-08-16 09:48:17
 * @LastEditors: zw_1520@163.com
 * @LastEditTime: 2025-08-26 10:25:11
 * Copyright (C) 2024-2050 Lens All rights reserved.
 */

#include "chassis_kinematics.h"

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_ros/transform_broadcaster.h>

#include <cmath>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>

#include "motor_control_utils.h"
static inline double wrapYaw(double a) {
  return std::atan2(std::sin(a), std::cos(a));
}

#ifndef RUNNING_PERIOD
#define RUNNING_PERIOD 10  // ms, default 100 Hz
#endif

constexpr double MAX_SPEED = 0.3;  // 最大速度

/**
 * @description: chassis kinematics constructor
 * @return {*}
 * @author: zw_1520@163.com
 */
ChassisKinematics::ChassisKinematics() : Node("chassis_kinematics") {
  // Inits wheel parameters
  m_wheel_distance_ = 0.5287;
  m_wheel_diameter_ = 0.173;

  m_current_odom_.x = 0;
  m_current_odom_.y = 0;
  m_current_odom_.theta = 0;
  m_current_odom_.vx = 0;
  m_current_odom_.v_theta = 0;

  m_last_left_wheel_circles_ = 0;
  m_last_right_wheel_circles_ = 0;
}

/**
 * @description: set chassis kinematics parameters
 * @return {*}
 * @author: zw_1520@163.com
 */
void ChassisKinematics::setKinematicsParameters(const double &wheel_distance) {
  m_wheel_distance_ = wheel_distance;
  return;
}

/**
 * @description: chassis forward kinematics calculation
 * @return {*}
 * @author: zw_1520@163.com
 */
void ChassisKinematics::kinematicForward(const double &linear_velocity,
                                         const double &angular_velocity,
                                         double &left_wheel_velocity,
                                         double &right_wheel_velocity) {
  left_wheel_velocity =
      (linear_velocity - angular_velocity * m_wheel_distance_ / 2);
  right_wheel_velocity =
      (linear_velocity + angular_velocity * m_wheel_distance_ / 2);
  return;
}

/**
 * @description: chassis invers kinematics calculation
 * @return {*}
 * @author: zw_1520@163.com
 */
void ChassisKinematics::kinematicInverse(const double &left_wheel_velocity,
                                         const double &right_wheel_velocity,
                                         double &linear_velocity,
                                         double &angular_velocity) {
  linear_velocity = (left_wheel_velocity + right_wheel_velocity) / 2;
  angular_velocity =
      (right_wheel_velocity - left_wheel_velocity) / m_wheel_distance_;
  return;
}

/**
 * @description: update odometry
 * @return {*}
 * @author: zw_1520@163.com
 */
Odom ChassisKinematics::updateOdom(double v_l, double v_r, double dt,
                                   double wheel_distance) {
  Odom odom;
  odom.x = 0;
  odom.y = 0;
  odom.theta = 0;
  odom.vx = 0;
  odom.v_theta = 0;

  // Calculates speed
  double v = (v_r + v_l) / 2.0;
  double omega = (v_r - v_l) / wheel_distance;
  if (fabs(v_r - v_l) < 0.02) {
    omega = 0;
  }
  // Checks robot move status
  double move_radius = 0;
  if (omega != 0) {
    move_radius = v / omega;  // Gets robot move circle radius

    // Updates robot position and orientation
    odom.theta = omega * dt;
    odom.x = move_radius * (sin(m_current_odom_.theta + odom.theta) -
                            sin(m_current_odom_.theta));
    odom.y = move_radius * (-cos(m_current_odom_.theta + odom.theta) +
                            cos(m_current_odom_.theta));
  } else {
    odom.x = v * dt * cos(m_current_odom_.theta);
    odom.y = v * dt * sin(m_current_odom_.theta);
    odom.theta = 0;
  }
  odom.vx = v;
  odom.v_theta = omega;
  return odom;
}

/**
 * @description: Calculates the minimum degree to other angles
 * @param {double} min_degree
 * @return {*}
 * @author: zw_1520@163.com
 */
RoberDegreeSort ChassisKinematics::minDegreeToOther(double min_degree) {
  RoberDegreeSort roberDegreeSort;
  // OP/PB =tan<OBP
  PB = OP / (tan(fabs(min_degree)));
  PH = PB - HB;
  PA = PB - AB;
  // OP/PH =tan<OHP
  // tan(_OHP)=OP/PH;
  roberDegreeSort.dCulMid = atan(OP / PH);

  // OP/PA =tan<OAP
  // tan(_OAP)=OP/PA;
  roberDegreeSort.dCulMax = atan(OP / PA);
  roberDegreeSort.dCulMin = min_degree;
  if (min_degree < 0) {
    roberDegreeSort.dCulMid = roberDegreeSort.dCulMid * (-1);
    roberDegreeSort.dCulMax = roberDegreeSort.dCulMax * (-1);
  }
  return roberDegreeSort;
}

/**
 * @description:  Calculates the maximum degree to other angles
 * @param {double} max_degree
 * @return {*}
 * @author: zw_1520@163.com
 */
RoberDegreeSort ChassisKinematics::maxDegreeToOther(double max_degree) {
  RoberDegreeSort roberDegreeSort;
  // OP/PB =tan<OBP
  PA = OP / (tan(fabs(max_degree)));
  PH = PA + AH;
  PB = PA + AB;
  // OP/PH =tan<OHP
  // tan(_OHP)=OP/PH;
  roberDegreeSort.dCulMid = atan(OP / PH);

  // OP/PA =tan<OAP
  // tan(_OAP)=OP/PA;
  roberDegreeSort.dCulMin = atan(OP / PB);
  roberDegreeSort.dCulMax = max_degree;
  if (max_degree < 0) {
    roberDegreeSort.dCulMid = roberDegreeSort.dCulMid * (-1);
    roberDegreeSort.dCulMin = roberDegreeSort.dCulMin * (-1);
  }
  return roberDegreeSort;
}

/**
 * @description:  Calculates the right wheel speed to other angles
 * @return {*}
 * @author: zw_1520@163.com
 */
RoberDegree ChassisKinematics::degreeRightToOther(
    double dRight)  // use<shy,get <rax、<tbz
{
  RoberDegree robot_turn_speed_now;
  //	double dTempDegree;
  RoberDegreeSort roberDegreeSort;
  if (dRight > 0) {
    roberDegreeSort = minDegreeToOther(dRight);
    robot_turn_speed_now._OAP_Left = roberDegreeSort.dCulMax;
    robot_turn_speed_now._OBP_Right = roberDegreeSort.dCulMin;
    robot_turn_speed_now._OHP_Mid = roberDegreeSort.dCulMid;
  } else {
    roberDegreeSort = maxDegreeToOther(dRight);
    robot_turn_speed_now._OAP_Left = roberDegreeSort.dCulMin;
    robot_turn_speed_now._OBP_Right = roberDegreeSort.dCulMax;
    robot_turn_speed_now._OHP_Mid = roberDegreeSort.dCulMid;
  }

  // OP/OA =SIN(_OAP)
  robot_turn_speed_now.OA_L = OP / sin((fabs(roberDegreeSort.dCulMax)));
  // OP/OB =SIN(_OBP)
  robot_turn_speed_now.OB_R = OP / sin((fabs(roberDegreeSort.dCulMin)));
  // OP/OH =SIN(_OHP)
  robot_turn_speed_now.OH_M = OP / sin((fabs(roberDegreeSort.dCulMid)));

  return robot_turn_speed_now;
}

/**
 * @description:  Calculates the left wheel speed to other angles
 * @return {*}
 * @author: zw_1520@163.com
 */
RoberDegree ChassisKinematics::degreeLeftToOther(
    double dLeft)  // use<shy,get <rax、<tbz
{
  RoberDegree robot_turn_speed_now;
  //	double dTempDegree;
  RoberDegreeSort roberDegreeSort;
  if (dLeft > 0) {
    roberDegreeSort = maxDegreeToOther(dLeft);
    robot_turn_speed_now._OAP_Left = roberDegreeSort.dCulMax;
    robot_turn_speed_now._OBP_Right = roberDegreeSort.dCulMin;
    robot_turn_speed_now._OHP_Mid = roberDegreeSort.dCulMid;
  } else {
    roberDegreeSort = minDegreeToOther(dLeft);
    robot_turn_speed_now._OAP_Left = roberDegreeSort.dCulMin;
    robot_turn_speed_now._OBP_Right = roberDegreeSort.dCulMax;
    robot_turn_speed_now._OHP_Mid = roberDegreeSort.dCulMid;
  }

  // OP/OA =SIN(_OAP)
  robot_turn_speed_now.OA_L = OP / sin((fabs(roberDegreeSort.dCulMax)));
  // OP/OB =SIN(_OBP)
  robot_turn_speed_now.OB_R = OP / sin((fabs(roberDegreeSort.dCulMin)));
  // OP/OH =SIN(_OHP)
  robot_turn_speed_now.OH_M = OP / sin((fabs(roberDegreeSort.dCulMid)));

  return robot_turn_speed_now;
}

/**
 * @description:  Calculates the right wheel speed to other angles
 * @return {*}
 * @author: zw_1520@163.com
 */
RoberDegree ChassisKinematics::degreeMiddleToOther(double middle_angle) {
  RoberDegree robot_turn_speed_now;
  double degree_max, degree_min;

  PH = OP / (tan(fabs(middle_angle)));
  PA = PH - AH;
  PB = PH + HB;
  degree_max = atan(OP / PA);
  degree_min = atan(OP / PB);
  if (middle_angle < 0) {
    robot_turn_speed_now._OHP_Mid = middle_angle;
    robot_turn_speed_now._OBP_Right = degree_max * (-1);
    robot_turn_speed_now._OAP_Left = degree_min * (-1);
  } else {
    robot_turn_speed_now._OHP_Mid = middle_angle;
    robot_turn_speed_now._OBP_Right = degree_min;
    robot_turn_speed_now._OAP_Left = degree_max;
  }

  // OP/OA =SIN(_OAP)
  robot_turn_speed_now.OA_L = OP / sin(degree_max);
  // OP/OB =SIN(_OBP)
  robot_turn_speed_now.OB_R = OP / sin(degree_min);
  // OP/OH =SIN(_OHP)
  robot_turn_speed_now.OH_M = OP / sin((fabs(middle_angle)));

  return robot_turn_speed_now;
}

/**
 * @description:  Calculates the robot wheel speed and angle by linear speed and
 * turn speed
 * @param {double} linear_speed
 * @param {double} turn_speed
 * @return {*}
 * @author: zw_1520@163.com
 */
void ChassisKinematics::doubleAckermannIK(double linear_speed,
                                          double turn_speed) {
  RCLCPP_INFO(this->get_logger(), "Robot turn_speed: %f, linear_speed: %f",
              turn_speed, linear_speed);
  if (linear_speed > MAX_SPEED) {
    linear_speed = MAX_SPEED;
  } else if (linear_speed < -MAX_SPEED) {
    linear_speed = -MAX_SPEED;
  }
  RoberDegree robot_turn_speed_now;
  static RoberDegree roberDegreeLast;

  robot_turn_speed_now = degreeMiddleToOther(turn_speed);

  if (turn_speed == 0) {
    RCLCPP_INFO(this->get_logger(), "Robot_turn_speed_now is zero: %f",
                turn_speed);
    wheel_speed.left_front_speed = linear_speed;
    wheel_speed.left_rear_speed = wheel_speed.left_front_speed;
    wheel_speed.right_front_speed = linear_speed;
    wheel_speed.right_rear_speed = wheel_speed.right_front_speed;
  } else {
    if (turn_speed < 0)  // right,left speed up
    {
      wheel_speed.left_front_speed =
          ((robot_turn_speed_now.OB_R * (((double)linear_speed))) /
           (fabs(cos(turn_speed)))) /
          robot_turn_speed_now.OH_M;
      wheel_speed.left_rear_speed = wheel_speed.left_front_speed;
      wheel_speed.right_front_speed =
          ((robot_turn_speed_now.OA_L * ((double)linear_speed)) /
           (fabs(cos(turn_speed)))) /
          robot_turn_speed_now.OH_M;
      wheel_speed.right_rear_speed = wheel_speed.right_front_speed;
    } else {
      wheel_speed.left_front_speed =
          ((robot_turn_speed_now.OA_L * ((double)linear_speed)) /
           (cos(turn_speed))) /
          robot_turn_speed_now.OH_M;
      wheel_speed.left_rear_speed = wheel_speed.left_front_speed;
      wheel_speed.right_front_speed =
          ((robot_turn_speed_now.OB_R * (((double)linear_speed))) /
           (cos(turn_speed))) /
          robot_turn_speed_now.OH_M;
      wheel_speed.right_rear_speed = wheel_speed.right_front_speed;
    }
  }

  wheel_speed.left_front_speed = wheel_speed.left_front_speed * (-1.0);
  wheel_speed.left_rear_speed = wheel_speed.left_rear_speed * (-1.0);

  // RCLCPP_INFO(this->get_logger(),
  //             " wheel_speed.left_front_speed: %f, "
  //             "wheel_speed.left_rear_speed: %f,
  //             wheel_speed.right_front_speed: "
  //             "%f, wheel_speed.right_rear_speed: %f",
  //             wheel_speed.left_front_speed, wheel_speed.left_rear_speed,
  //             wheel_speed.right_front_speed, wheel_speed.right_rear_speed);

  // RCLCPP_INFO(
  //     this->get_logger(),
  //     " robot_turn_speed_now._OAP_Left: %f, robot_turn_speed_now._OHP_Mid: "
  //     "%f, robot_turn_speed_now._OBP_Right: %f, OA_L: %f, OH_M: %f, OB_R:
  //     %f", robot_turn_speed_now._OAP_Left, robot_turn_speed_now._OHP_Mid,
  //     robot_turn_speed_now._OBP_Right, robot_turn_speed_now.OA_L,
  //     robot_turn_speed_now.OH_M, robot_turn_speed_now.OB_R);

  roberDegreeLast = robot_turn_speed_now;
  RobotWheelAngle wheel_angle;
  wheel_angle.left_front_angle = robot_turn_speed_now._OAP_Left;
  wheel_angle.left_rear_angle = robot_turn_speed_now._OAP_Left * (-1.0);
  wheel_angle.right_front_angle = robot_turn_speed_now._OBP_Right;
  wheel_angle.right_rear_angle = robot_turn_speed_now._OBP_Right * (-1.0);

  // RCLCPP_INFO(this->get_logger(),
  //             "cmd_vel to wheel wheel_angle.left_front_angle: %f, "
  //             "wheel_angle.left_rear_angle: %f,
  //             wheel_angle.right_front_angle: "
  //             "%f, wheel_angle.right_rear_angle: %f",
  //             wheel_angle.left_front_angle, wheel_angle.left_rear_angle,
  //             wheel_angle.right_front_angle, wheel_angle.right_rear_angle);

  MotorControlUtils::getController()[MotorType::FRONT_LEFT_POWER_MOTOR]
      ->setTargetVelocity(wheel_speed.left_front_speed);
  MotorControlUtils::getController()[MotorType::FRONT_LEFT_STEERING_MOTOR]
      ->setAbsPosition(wheel_angle.left_front_angle);
  MotorControlUtils::getController()[MotorType::FRONT_RIGHT_POWER_MOTOR]
      ->setTargetVelocity(wheel_speed.right_front_speed);
  MotorControlUtils::getController()[MotorType::FRONT_RIGHT_STEERING_MOTOR]
      ->setAbsPosition(wheel_angle.right_front_angle);
  MotorControlUtils::getController()[MotorType::REAR_LEFT_POWER_MOTOR]
      ->setTargetVelocity(wheel_speed.left_rear_speed);
  MotorControlUtils::getController()[MotorType::REAR_LEFT_STEERING_MOTOR]
      ->setAbsPosition(wheel_angle.left_rear_angle);
  MotorControlUtils::getController()[MotorType::REAR_RIGHT_POWER_MOTOR]
      ->setTargetVelocity(wheel_speed.right_rear_speed);
  MotorControlUtils::getController()[MotorType::REAR_RIGHT_STEERING_MOTOR]
      ->setAbsPosition(wheel_angle.right_rear_angle);
}

/**
 * @description:  Calculates the robot wheel angle by wheel speed and wheel
 * speed
 * @param {RobotWheelSpeed} wheel_speed
 * @param {RobotWheelAngle} wheel_angle
 * @return {*}
 * @author: zw_1520@163.com
 */
void ChassisKinematics::doubleAckermannFK(
    const RobotWheelSpeed &wheel_speed, const RobotWheelAngle &wheel_angle,
    geometry_msgs::msg::Twist &robot_speed) {
  // RCLCPP_INFO(
  //     this->get_logger(),
  //     "doubleAckermannFK wheel_speed.left_front_speed: %f, "
  //     "wheel_speed.left_rear_speed: "
  //     "%f, wheel_speed.right_front_speed: %f, wheel_speed.right_rear_speed: "
  //     "%f",
  //     wheel_speed.left_front_speed, wheel_speed.left_rear_speed,
  //     wheel_speed.right_front_speed, wheel_speed.right_rear_speed);

  // RCLCPP_INFO(this->get_logger(),
  //             "doubleAckermannFK wheel_angle.left_front_angle: %f, "
  //             "wheel_angle.left_rear_angle: "
  //             "%f, wheel_angle.right_front_angle: %f, "
  //             "wheel_angle.right_rear_angle: %f",
  //             wheel_angle.left_front_angle, wheel_angle.left_rear_angle,
  //             wheel_angle.right_front_angle, wheel_angle.right_rear_angle);

  RoberDegree degree_temp = degreeLeftToOther(wheel_angle.left_front_angle);
  // RCLCPP_INFO(rclcpp::get_logger("rclcpp"), "degree_temp._OHP_Mid: %f",
  //             degree_temp._OHP_Mid);

  robot_speed.angular.z =
      degree_temp._OHP_Mid;  // Use the mid angle for z_speed

  if (abs(robot_speed.angular.z) < 0.02) {
    robot_speed.linear.x = wheel_speed.right_front_speed;
  } else {
    if (robot_speed.angular.z > 0) {
      robot_speed.linear.x =
          (((wheel_speed.right_front_speed * degree_temp.OH_M)) /
           degree_temp.OB_R) *
          (cos(degree_temp._OHP_Mid));
    } else {
      robot_speed.linear.x =
          (((wheel_speed.left_front_speed * degree_temp.OH_M)) /
           degree_temp.OB_R) *
          (cos(degree_temp._OHP_Mid));
    }
  }

  // RCLCPP_INFO(this->get_logger(),
  //             "robot_speed.linear.x: %f, robot_speed.angular.z: %f",
  //             robot_speed.linear.x, robot_speed.angular.z);
}
void ChassisKinematics::timer() {
  RobotWheelSpeed wheel_speed;
  RobotWheelAngle wheel_angle;

  geometry_msgs::msg::Twist robot_speed;
  // Odometry publish (50-100Hz driven by RUNNING_PERIOD)
  // 确保发布器和广播器只创建一次
  rclcpp::Publisher<nav_msgs::msg::Odometry>::SharedPtr odom_pub;
  std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster;
  if (!odom_pub)
    odom_pub = this->create_publisher<nav_msgs::msg::Odometry>(
        "/odom", rclcpp::SensorDataQoS());
  if (!tf_broadcaster)
    tf_broadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);

  rclcpp::Time last_stamp, now;
  while (rclcpp::ok()) {
    // 阻塞直到下一个时间周期
    wheel_speed.left_front_speed =
        MotorControlUtils::getController()[MotorType::FRONT_LEFT_POWER_MOTOR]
            ->getLinear();
    wheel_angle.left_front_angle =
        MotorControlUtils::getController()[MotorType::FRONT_LEFT_STEERING_MOTOR]
            ->getAngular();
    wheel_speed.right_front_speed =
        MotorControlUtils::getController()[MotorType::FRONT_RIGHT_POWER_MOTOR]
            ->getLinear();
    wheel_angle.right_front_angle = MotorControlUtils::getController()
                                        [MotorType::FRONT_RIGHT_STEERING_MOTOR]
                                            ->getAngular();
    wheel_speed.left_rear_speed =
        MotorControlUtils::getController()[MotorType::REAR_LEFT_POWER_MOTOR]
            ->getLinear();
    wheel_angle.left_rear_angle =
        MotorControlUtils::getController()[MotorType::REAR_LEFT_STEERING_MOTOR]
            ->getAngular();
    wheel_speed.right_rear_speed =
        MotorControlUtils::getController()[MotorType::REAR_RIGHT_POWER_MOTOR]
            ->getLinear();
    wheel_angle.right_rear_angle =
        MotorControlUtils::getController()[MotorType::REAR_RIGHT_STEERING_MOTOR]
            ->getAngular();
    doubleAckermannFK(wheel_speed, wheel_angle, robot_speed);
    last_stamp = this->get_clock()->now();

    std::this_thread::sleep_for(std::chrono::milliseconds(RUNNING_PERIOD));
    now = this->get_clock()->now();
    double dt = (now - last_stamp).seconds();
    if (dt <= 0) {
      dt = RUNNING_PERIOD / 1000.0;
    }
    // 获取车体速度（根据实际数据源替换）
    const double v =
        robot_speed.linear.x;  // 或者 wheel_speed.linear，根据定义调整
    const double w =
        robot_speed.angular.z;  // 或者 wheel_angle.angular，根据定义调整

    // 积分更新位置
    pose_.yaw += w * dt;
    if (pose_.yaw >= 2 * M_PI )
    {
      pose_.yaw-=2 * M_PI;
    }
    else if (pose_.yaw <= -2.0 * M_PI)
    {
      pose_.yaw +=2 * M_PI;
    }
    
    pose_.x += v * std::cos(pose_.yaw) * dt;
    pose_.y += v * std::sin(pose_.yaw) * dt;
    nav_msgs::msg::Odometry odom;
    odom.header.stamp = now;
    odom.header.frame_id = "odom";
    odom.child_frame_id = "base_link";
    odom.pose.pose.position.x = pose_.x;
    odom.pose.pose.position.y = pose_.y;
    odom.pose.pose.position.z = 0.0;
    tf2::Quaternion q;
    q.setRPY(0.0, 0.0, pose_.yaw);
    odom.pose.pose.orientation = tf2::toMsg(q);
    odom.twist.twist.linear.x = v;
    odom.twist.twist.angular.z = w;
    odom_pub->publish(odom);

    geometry_msgs::msg::TransformStamped tf_msg;
    tf_msg.header.stamp = now;
    tf_msg.header.frame_id = "odom";
    tf_msg.child_frame_id = "base_link";
    tf_msg.transform.translation.x = pose_.x;
    tf_msg.transform.translation.y = pose_.y;
    tf_msg.transform.translation.z = 0.0;
    tf_msg.transform.rotation = odom.pose.pose.orientation;
    tf_broadcaster->sendTransform(tf_msg);
  }
}